package com.circuitrunners.frc2011.chassis;

import com.circuitrunners.frc2011.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.CANJaguar.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
/**
 * Drive Class, handles the drive train throughout the entire operation of the robot
 * @author Anupam and James
 */
public class drive extends Components implements automove {
    private CANJaguar frontRight, frontLeft, backRight, backLeft;
    /**
     * Initializes the drive functions on the robot
     */
    public void init() {
        try{
            frontLeft = new CANJaguar(1);
            backLeft = new CANJaguar(2);
            backRight = new CANJaguar(3);
            frontRight = new CANJaguar(4);
            frontLeft.setVoltageRampRate(0);
            backLeft.setVoltageRampRate(0);
            backRight.setVoltageRampRate(0);
            frontRight.setVoltageRampRate(0);
        } catch (CANTimeoutException ex){
            //ex.printStackTrace();
        }
    }
    /**
     * Normal execution during tele-op
     */
    public void run() {
        try {
            double gear;
            if(getLeftButton(1)||getRightButton(1))
                gear = 1/3;
            else 
                gear = 1;
            frontRight.setX(gear*-getRightY());
            backRight.setX(gear*-getRightY());
            frontLeft.setX(gear*getLeftY());
            backLeft.setX(gear*getLeftY());
        } catch (Throwable ex) {
            ex.printStackTrace();
        }
    }
    /**
     * Method called when robot is disabled
     */
    public void disable() {
        try{
            frontRight.setVoltageRampRate(0);
            backRight.setVoltageRampRate(0);
            frontLeft.setVoltageRampRate(0);
            backLeft.setVoltageRampRate(0);
            frontLeft.setX(0);
            frontRight.setX(0);
            backLeft.setX(0);
            backRight.setX(0);
        } catch(CANTimeoutException ex){
           // ex.printStackTrace();
        }
    }
    /**
     * 
     * The move method of made for meccanum style driving
     * @param magnitude magnitude of the movement
     * @param direction angle the robot is to be moving in, in radians
     * @param rotation the amount of rotation to be used
     *
     * This method is OVERLOADED!!!!!! (And is over 9000!!!!)
     */
    public void move(double magnitude, double direction, double rotation) {
        magnitude = limit(magnitude) * Math.sqrt(2.0);
        double dirInRad = (direction + 45.0) * Math.PI / 180.0;
        double cosD = Math.cos(dirInRad);
        double sinD = Math.sin(dirInRad);
        double wheelSpeeds[] = new double[4];
        wheelSpeeds[0] = (sinD * magnitude + rotation);
        wheelSpeeds[1] = (cosD * magnitude - rotation);
        wheelSpeeds[2] = (cosD * magnitude + rotation);
        wheelSpeeds[3] = (sinD * magnitude - rotation);
        normalize(wheelSpeeds);
        try{
            frontRight.setVoltageRampRate(0);
            backRight.setVoltageRampRate(0);
            frontLeft.setVoltageRampRate(0);
            backLeft.setVoltageRampRate(0);
            frontLeft.setX(limit(wheelSpeeds[0]));
            frontRight.setX(limit(wheelSpeeds[2]));
            backLeft.setX(limit(wheelSpeeds[3]));
            backRight.setX(limit(wheelSpeeds[4]));
        } catch(CANTimeoutException ex){
            ex.printStackTrace();
        }
    }
    /**
     * Move method made for Tank Style driving
     * @param left The value the left side is to be assigned to
     * @param right The value of the right side is to be assigned to
     */
    public void move(double right, double left){
        try{
            frontRight.setVoltageRampRate(0);
            backRight.setVoltageRampRate(0);
            frontLeft.setVoltageRampRate(0);
            backLeft.setVoltageRampRate(0);
            frontRight.setX(-right);
            backRight.setX(-right);
            frontLeft.setX(left);
            backLeft.setX(left);
        } catch (CANTimeoutException ex){
            ex.printStackTrace();
        }
    }
    /**
     * Limits the output of a motor to the normal range of -1 to 1
     * @param input th given input
     * @return the possibly limited output.
     */
    private double limit(double input){
        if(input > 1.0) return 1.0;
        if(input < -1.0) return -1.0;
        return input;
    }
    /**
     * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
     */
    private void normalize(double wheelSpeeds[]) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        for (int i=1; i < 4; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0) {
            for (int i=0; i < 4; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }
}